#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/bool.hpp>
#include <serial/serial.h>  // 引入 serial 库
#include <iomanip>
class GPIODriver : public rclcpp::Node
{
public:
    GPIODriver() : Node("gpio_driver")
    {
        RCLCPP_WARN(this->get_logger(), "starting GPIO driver node");

        // 从参数服务器获取 gpio_port_name
        // this->declare_parameter<std::string>("gpio_port_name", "/dev/ttyGPIO"); // 默认串口
        this->declare_parameter<std::string>("gpio_port_name", "/dev/ttyUSB0"); // 默认串口

        this->get_parameter("gpio_port_name", gpio_port_name_);

        // 打开串口
        open_serial_port();

        // 定时器
        read_timer_ = this->create_wall_timer(
            std::chrono::duration<double>(1.0 / 300.0), std::bind(&GPIODriver::timer_callback, this));

        open_serial_timer_ = this->create_wall_timer(
            std::chrono::duration<double>(1.0), std::bind(&GPIODriver::open_serial_port_timer_callback, this));

        // 创建发布器
        switch_pub = this->create_publisher<std_msgs::msg::Bool>("/gpio_driver/switch_state", 1000);
        switch_trigger_pub = this->create_publisher<std_msgs::msg::Bool>("/gpio_driver/switch_trigger", 1000);
        
    }


private:
    std::string gpio_port_name_;
    serial::Serial ser;  // 使用 serial 库管理串口
    rclcpp::TimerBase::SharedPtr read_timer_,open_serial_timer_;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr switch_pub;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr switch_trigger_pub;
    bool last_switch_value = false;
    int pack_length = 3;



    void open_serial_port()
    {
        try
        {
         // 如果串口已打开，先关闭
            if (ser.isOpen()) {
                RCLCPP_WARN(this->get_logger(), "Serial port is already open. Closing it first.");
                ser.close();
            }
            ser.setPort(gpio_port_name_);

            RCLCPP_INFO(this->get_logger(), "GPIO openning port: %s", gpio_port_name_.c_str());

            ser.setBaudrate(115200);
            serial::Timeout to = serial::Timeout::simpleTimeout(1000);
            ser.setTimeout(to);

            ser.open();
        }

        catch (const serial::IOException &e)
        {
            RCLCPP_ERROR(this->get_logger(), "Unable to open port %s", gpio_port_name_.c_str());
        }
        catch (const std::exception &e)
        {
            RCLCPP_ERROR(this->get_logger(), "Exception: %s", e.what());
        }
        ClearSerialBuffer();
    }

    void ClearSerialBuffer()
    {
        RCLCPP_WARN(this->get_logger(), "data is blocked!! clear buffer");
        if (!ser.isOpen())
        {
            std::cerr << "Serial port is not open." << std::endl;
            return;
        }

        std::vector<uint8_t> buffer(ser.available());
        ser.read(buffer, buffer.size());

        RCLCPP_WARN(this->get_logger(), "data is blocked!! clear buffer:");
        printHex(buffer);

        // You might also want to write a dummy byte and then flush the output buffer
        ser.flushOutput();
        // ROS_INFO("Serial buffer of %d bytes has been cleared.",count);
    }
    void printHex(const std::vector<uint8_t> &data)
    {
        std::stringstream ss;
        ss << std::hex << std::setw(2) << std::setfill('0');
        for (std::size_t i = 0; i < data.size(); ++i)
        {
            ss << std::setw(2) << static_cast<int>(data[i]) << ' ';
        }
        RCLCPP_INFO(this->get_logger(), ss.str().c_str());
    }
    
    void open_serial_port_timer_callback()
    {
        if (!ser.isOpen())
        {
            open_serial_port();
        }
    }

    void decode_tty_data(std::vector<uint8_t> tty_buffer)
    {

        // std::vector<uint8_t> valid_data;
        // for (size_t i = 0; i < bytes_read; ++i) {
        //     if (buffer[i] != 0x00) {  // 忽略0x00字节
        //         valid_data.push_back(buffer[i]);
        //     }
        // }
        // RCLCPP_INFO(this->get_logger(), "decoding: ");
        // printHex(tty_buffer);
        // 判断返回的数据格式，如 2F 01 01/00 表示高低电平
        uint8_t header = tty_buffer[0];
        if(!header == 0x2F)
        {
            RCLCPP_WARN(this->get_logger(), "header error! %c", header);
            return;
        }

        bool current_switch_value = !(tty_buffer[2] == 0x01);  // 如果返回的是 0x01 表示开
        // RCLCPP_INFO(this->get_logger(), "read data %d", current_switch_value);

        // 发布 switch 状态
        std_msgs::msg::Bool switch_msg;
        switch_msg.data = current_switch_value;
        switch_pub->publish(switch_msg);

        // 判断开关状态变化，发布触发信号
        if (last_switch_value == false && current_switch_value == true)
        {
            static int trigger_count = 0;
            trigger_count++;
            RCLCPP_INFO(this->get_logger(), "trigger count %d", trigger_count);
            RCLCPP_INFO(this->get_logger(), "圈数： %f", (double)trigger_count/100.0);

            std_msgs::msg::Bool trigger_msg;
            trigger_msg.data = true;
            switch_trigger_pub->publish(trigger_msg);
        }
        else if (last_switch_value == true && current_switch_value == false)
        {
            std_msgs::msg::Bool trigger_msg;
            trigger_msg.data = false;
            switch_trigger_pub->publish(trigger_msg);
        }

        last_switch_value = current_switch_value;
    }


    void timer_callback(){
        if (ser.isOpen())
        {
            try
            {
                ser.write("\x3F\x01");
                // 读取串口返回的数据
                std::vector<uint8_t> buffer(0);
                // uint8_t buffer[100];

                int available_length = ser.available();
                // RCLCPP_INFO(this->get_logger(), "available length %d", available_length);

                // size_t bytes_read = ser.read(buffer, buffer.size());

                size_t bytes_read = 0;
                if(available_length > 0)
                    bytes_read = ser.read(buffer, available_length);
                else
                    return;

                if (bytes_read == pack_length)
                {
                    decode_tty_data(buffer);
                }
                else
                {
                    RCLCPP_WARN(this->get_logger(), "read data length error! %d", bytes_read);
                    printHex(buffer);
                    ClearSerialBuffer();
                }
            }
            catch (const serial::SerialException &e) // 捕获串口相关的异常
            {
                RCLCPP_ERROR(this->get_logger(), "SerialException: %s", e.what());
            }
            catch (const std::exception &e) // 捕获通用异常
            {
                RCLCPP_ERROR(this->get_logger(), "Exception: %s", e.what());
            }
        }
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<GPIODriver>());
    rclcpp::shutdown();
    return 0;
}
